package jp.seraph.jsade.model.nao;

import java.util.HashMap;
import java.util.Map;

import org.apache.commons.math.geometry.Vector3D;

import jp.seraph.jsade.message.MessageUtil;
import jp.seraph.jsade.model.DefaultJoint;
import jp.seraph.jsade.model.DefaultModelPart;
import jp.seraph.jsade.model.Joint;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jsade.model.ModelObjectIdentifier;
import jp.seraph.jsade.model.ModelPart;
import jp.seraph.jsade.model.ObjectContainer;

/**
 * Naoのオブジェクトの生成・組み立て・格納を行うクラスです。 Naoのオブジェクト構成に従い、JointとObjectの接続を行います。
 *
 */
public class NaoObjectContainer implements ObjectContainer {
    public NaoObjectContainer() {
        mJointMap = new HashMap<NaoJointIdentifier, Joint>();
        mObjectMap = new HashMap<NaoObjectIdentifier, ModelPart>();
    }

    private Map<NaoJointIdentifier, Joint> mJointMap;
    private Map<NaoObjectIdentifier, ModelPart> mObjectMap;

    public Joint getJoint(JointIdentifier aIdentifier) {
        Joint tResult = mJointMap.get(aIdentifier);
        if (tResult == null) {
            createJoint((NaoJointIdentifier) aIdentifier);
            return mJointMap.get(aIdentifier);
        }

        return tResult;
    }

    public ModelPart getObject(ModelObjectIdentifier aIdentifier) {
        ModelPart tResult = mObjectMap.get(aIdentifier);
        if (tResult == null) {
            createObject((NaoObjectIdentifier) aIdentifier);
            return mObjectMap.get(aIdentifier);
        }

        return tResult;
    }

    private void createJoint(NaoJointIdentifier aIdentifier) {
        DefaultJoint tNewJoint = new DefaultJoint();
        mJointMap.put(aIdentifier, tNewJoint);

        if (aIdentifier == NaoJointIdentifier.HJ1) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.TORSO), this.getObject(NaoObjectIdentifier.NECK), new Vector3D(0, 0, 0.09), new Vector3D(0, 0, 1));
        } else if (aIdentifier == NaoJointIdentifier.HJ2) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.NECK), this.getObject(NaoObjectIdentifier.HEAD), new Vector3D(0, 0, 0.060), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.LAJ1) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.TORSO), this.getObject(NaoObjectIdentifier.LEFT_SHOULDER), new Vector3D(-0.098, 0, 0.075), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.LAJ2) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_SHOULDER), this.getObject(NaoObjectIdentifier.LEFT_UPPER_ARM), Vector3D.zero, new Vector3D(0, 0, 1));
        } else if (aIdentifier == NaoJointIdentifier.LAJ3) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_UPPER_ARM), this.getObject(NaoObjectIdentifier.LEFT_ELBOW), new Vector3D(0.01, 0.07, 0.009), new Vector3D(0, 1, 0));
        } else if (aIdentifier == NaoJointIdentifier.LAJ4) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_ELBOW), this.getObject(NaoObjectIdentifier.LEFT_LOWER_ARM), Vector3D.zero, new Vector3D(0, 0, 1));
        } else if (aIdentifier == NaoJointIdentifier.RAJ1) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.TORSO), this.getObject(NaoObjectIdentifier.RIGHT_SHOULDER), new Vector3D(0.098, 0, 0.075), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.RAJ2) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_SHOULDER), this.getObject(NaoObjectIdentifier.RIGHT_UPPER_ARM), Vector3D.zero, new Vector3D(0, 0, 1));
        } else if (aIdentifier == NaoJointIdentifier.RAJ3) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_UPPER_ARM), this.getObject(NaoObjectIdentifier.RIGHT_ELBOW), new Vector3D(-0.01, 0.07, 0.009), new Vector3D(0, 1, 0));
        } else if (aIdentifier == NaoJointIdentifier.RAJ4) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_ELBOW), this.getObject(NaoObjectIdentifier.RIGHT_LOWER_ARM), Vector3D.zero, new Vector3D(0, 0, 1));
        } else if (aIdentifier == NaoJointIdentifier.LLJ1) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.TORSO), this.getObject(NaoObjectIdentifier.LEFT_HIP1), new Vector3D(-0.055, -0.01, -0.115), new Vector3D(-0.7071, 0, -0.7071));
        } else if (aIdentifier == NaoJointIdentifier.LLJ2) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_HIP1), this.getObject(NaoObjectIdentifier.LEFT_HIP2), Vector3D.zero, new Vector3D(0, 1, 0));
        } else if (aIdentifier == NaoJointIdentifier.LLJ3) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_HIP2), this.getObject(NaoObjectIdentifier.LEFT_THIGH), Vector3D.zero, new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.LLJ4) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_THIGH), this.getObject(NaoObjectIdentifier.LEFT_SHANK), new Vector3D(0, -0.005, -0.08), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.LLJ5) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_SHANK), this.getObject(NaoObjectIdentifier.LEFT_ANKLE), new Vector3D(0, -0.01, -0.055), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.LLJ6) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.LEFT_ANKLE), this.getObject(NaoObjectIdentifier.LEFT_FOOT), Vector3D.zero, new Vector3D(0, 1, 0));
        } else if (aIdentifier == NaoJointIdentifier.RLJ1) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.TORSO), this.getObject(NaoObjectIdentifier.RIGHT_HIP1), new Vector3D(0.055, -0.01, -0.115), new Vector3D(-0.7071, 0, 0.7071));
        } else if (aIdentifier == NaoJointIdentifier.RLJ2) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_HIP1), this.getObject(NaoObjectIdentifier.RIGHT_HIP2), Vector3D.zero, new Vector3D(0, 1, 0));
        } else if (aIdentifier == NaoJointIdentifier.RLJ3) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_HIP2), this.getObject(NaoObjectIdentifier.RIGHT_THIGH), Vector3D.zero, new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.RLJ4) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_THIGH), this.getObject(NaoObjectIdentifier.RIGHT_SHANK), new Vector3D(0, -0.005, -0.08), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.RLJ5) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_SHANK), this.getObject(NaoObjectIdentifier.RIGHT_ANKLE), new Vector3D(0, -0.01, -0.055), new Vector3D(1, 0, 0));
        } else if (aIdentifier == NaoJointIdentifier.RLJ6) {
            tNewJoint.init(aIdentifier, aIdentifier.toJointName(), this.getObject(NaoObjectIdentifier.RIGHT_ANKLE), this.getObject(NaoObjectIdentifier.RIGHT_FOOT), Vector3D.zero, new Vector3D(0, 1, 0));
        } else {
            mJointMap.remove(aIdentifier);
            throw new AssertionError(MessageUtil.enumAssertMessage("NaoJointIdentifier"));
        }
    }

    private void createObject(NaoObjectIdentifier aIdentifier) {
        DefaultModelPart tNewParts = new DefaultModelPart();
        mObjectMap.put(aIdentifier, tNewParts);

        if (aIdentifier == NaoObjectIdentifier.TORSO) {
            tNewParts.init(aIdentifier.toObjectName(), 1.2171, Vector3D.zero, this.getJoint(NaoJointIdentifier.HJ1), this.getJoint(NaoJointIdentifier.LAJ1), this.getJoint(NaoJointIdentifier.RAJ1), this.getJoint(NaoJointIdentifier.LLJ1), this.getJoint(NaoJointIdentifier.RLJ1));
        } else if (aIdentifier == NaoObjectIdentifier.HEAD) {
            tNewParts.init(aIdentifier.toObjectName(), 0.35, new Vector3D(0, 0, 0.005), this.getJoint(NaoJointIdentifier.HJ2));
        } else if (aIdentifier == NaoObjectIdentifier.NECK) {
            tNewParts.init(aIdentifier.toObjectName(), 0.05, new Vector3D(0, 0, 0.09), this.getJoint(NaoJointIdentifier.HJ1), this.getJoint(NaoJointIdentifier.HJ2));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_ANKLE) {
            tNewParts.init(aIdentifier.toObjectName(), 0.125, Vector3D.zero, this.getJoint(NaoJointIdentifier.LLJ5), this.getJoint(NaoJointIdentifier.LLJ6));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_ELBOW) {
            tNewParts.init(aIdentifier.toObjectName(), 0.035, Vector3D.zero, this.getJoint(NaoJointIdentifier.LAJ3), this.getJoint(NaoJointIdentifier.LAJ4));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_FOOT) {
            tNewParts.init(aIdentifier.toObjectName(), 0.2, new Vector3D(0, 0.03, -0.035), this.getJoint(NaoJointIdentifier.LLJ6));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_HIP1) {
            tNewParts.init(aIdentifier.toObjectName(), 0.09, Vector3D.zero, this.getJoint(NaoJointIdentifier.LLJ1), this.getJoint(NaoJointIdentifier.LLJ2));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_HIP2) {
            tNewParts.init(aIdentifier.toObjectName(), 0.125, Vector3D.zero, this.getJoint(NaoJointIdentifier.LLJ2), this.getJoint(NaoJointIdentifier.LLJ3));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_LOWER_ARM) {
            tNewParts.init(aIdentifier.toObjectName(), 0.2, new Vector3D(0, 0.05, 0), this.getJoint(NaoJointIdentifier.LAJ4));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_SHANK) {
            tNewParts.init(aIdentifier.toObjectName(), 0.225, new Vector3D(0, 0.01, 0.045), this.getJoint(NaoJointIdentifier.LLJ4), this.getJoint(NaoJointIdentifier.LLJ5));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_SHOULDER) {
            tNewParts.init(aIdentifier.toObjectName(), 0.07, Vector3D.zero, this.getJoint(NaoJointIdentifier.LAJ1), this.getJoint(NaoJointIdentifier.LAJ2));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_THIGH) {
            tNewParts.init(aIdentifier.toObjectName(), 0.275, new Vector3D(0, 0.01, -0.04), this.getJoint(NaoJointIdentifier.LLJ3), this.getJoint(NaoJointIdentifier.LLJ4));
        } else if (aIdentifier == NaoObjectIdentifier.LEFT_UPPER_ARM) {
            tNewParts.init(aIdentifier.toObjectName(), 0.15, new Vector3D(0, 0.01, -0.04), this.getJoint(NaoJointIdentifier.LAJ2), this.getJoint(NaoJointIdentifier.LAJ3));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_ANKLE) {
            tNewParts.init(aIdentifier.toObjectName(), 0.125, Vector3D.zero, this.getJoint(NaoJointIdentifier.RLJ5), this.getJoint(NaoJointIdentifier.RLJ6));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_ELBOW) {
            tNewParts.init(aIdentifier.toObjectName(), 0.035, Vector3D.zero, this.getJoint(NaoJointIdentifier.RAJ3), this.getJoint(NaoJointIdentifier.RAJ4));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_FOOT) {
            tNewParts.init(aIdentifier.toObjectName(), 0.2, new Vector3D(0, 0.03, -0.035), this.getJoint(NaoJointIdentifier.RLJ6));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_HIP1) {
            tNewParts.init(aIdentifier.toObjectName(), 0.09, Vector3D.zero, this.getJoint(NaoJointIdentifier.RLJ1), this.getJoint(NaoJointIdentifier.RLJ2));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_HIP2) {
            tNewParts.init(aIdentifier.toObjectName(), 0.125, Vector3D.zero, this.getJoint(NaoJointIdentifier.RLJ2), this.getJoint(NaoJointIdentifier.RLJ3));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_LOWER_ARM) {
            tNewParts.init(aIdentifier.toObjectName(), 0.2, new Vector3D(0, 0.05, 0), this.getJoint(NaoJointIdentifier.RAJ4));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_SHANK) {
            tNewParts.init(aIdentifier.toObjectName(), 0.225, new Vector3D(0, 0.01, 0.045), this.getJoint(NaoJointIdentifier.RLJ4), this.getJoint(NaoJointIdentifier.RLJ5));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_SHOULDER) {
            tNewParts.init(aIdentifier.toObjectName(), 0.07, Vector3D.zero, this.getJoint(NaoJointIdentifier.RAJ1), this.getJoint(NaoJointIdentifier.RAJ2));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_THIGH) {
            tNewParts.init(aIdentifier.toObjectName(), 0.275, new Vector3D(0, 0.01, -0.04), this.getJoint(NaoJointIdentifier.RLJ3), this.getJoint(NaoJointIdentifier.RLJ4));
        } else if (aIdentifier == NaoObjectIdentifier.RIGHT_UPPER_ARM) {
            tNewParts.init(aIdentifier.toObjectName(), 0.15, new Vector3D(0, 0.01, -0.04), this.getJoint(NaoJointIdentifier.RAJ2), this.getJoint(NaoJointIdentifier.RAJ3));
        } else {
            mObjectMap.remove(aIdentifier);
            throw new AssertionError(MessageUtil.enumAssertMessage("NaoObjectIdentifier"));
        }
    }
}
